﻿// Decompiled with JetBrains decompiler
// Type: DnProcessSimulateCommands.ModularFixture.CApMFTransformation
// Assembly: DnProcessSimulateCommands, Version=2402.0.0.890, Culture=neutral, PublicKeyToken=null
// MVID: A885D881-AEDF-447E-852A-6F13102E089C
// Assembly location: C:\Program Files\Tecnomatix_2402\eMPower\DotNetCommands\DnProcessSimulateCommands.dll

using System;
using Tecnomatix.Engineering;


namespace DnProcessSimulateCommands.ModularFixture;

internal class CApMFTransformation
{
  private const double RTEpsilonD = 1E-06;
  internal double[][] m_tranData;

  internal CApMFTransformation()
  {
    this.m_tranData = new double[4][];
    for (int index = 0; index < 4; ++index)
      this.m_tranData[index] = new double[4];
    this.m_tranData[0][0] = this.m_tranData[1][1] = this.m_tranData[2][2] = this.m_tranData[3][3] = 1.0;
    this.m_tranData[0][1] = this.m_tranData[0][2] = this.m_tranData[0][3] = this.m_tranData[1][0] = this.m_tranData[1][2] = this.m_tranData[1][3] = this.m_tranData[2][0] = this.m_tranData[2][1] = this.m_tranData[2][3] = this.m_tranData[3][0] = this.m_tranData[3][1] = this.m_tranData[3][2] = 0.0;
  }

  internal CApMFTransformation(CApMFTransformation other)
  {
    this.m_tranData = new double[4][];
    for (int index = 0; index < 4; ++index)
      this.m_tranData[index] = new double[4];
    this.m_tranData[0][0] = other.m_tranData[0][0];
    this.m_tranData[0][1] = other.m_tranData[0][1];
    this.m_tranData[0][2] = other.m_tranData[0][2];
    this.m_tranData[0][3] = other.m_tranData[0][3];
    this.m_tranData[1][0] = other.m_tranData[1][0];
    this.m_tranData[1][1] = other.m_tranData[1][1];
    this.m_tranData[1][2] = other.m_tranData[1][2];
    this.m_tranData[1][3] = other.m_tranData[1][3];
    this.m_tranData[2][0] = other.m_tranData[2][0];
    this.m_tranData[2][1] = other.m_tranData[2][1];
    this.m_tranData[2][2] = other.m_tranData[2][2];
    this.m_tranData[2][3] = other.m_tranData[2][3];
    this.m_tranData[3][0] = other.m_tranData[3][0];
    this.m_tranData[3][1] = other.m_tranData[3][1];
    this.m_tranData[3][2] = other.m_tranData[3][2];
    this.m_tranData[3][3] = other.m_tranData[3][3];
  }

  internal CApMFTransformation(double x, double y, double z, double rx, double ry, double rz)
  {
    this.m_tranData = new double[4][];
    for (int index = 0; index < 4; ++index)
      this.m_tranData[index] = new double[4];
    this.m_tranData[3][0] = 0.0;
    this.m_tranData[3][1] = 0.0;
    this.m_tranData[3][2] = 0.0;
    this.m_tranData[3][3] = 1.0;
    this.SetRotationRPY(rx, ry, rz);
    this.SetTranslation(x, y, z);
  }

  internal void Set(double x, double y, double z, double rx, double ry, double rz)
  {
    this.m_tranData[3][0] = 0.0;
    this.m_tranData[3][1] = 0.0;
    this.m_tranData[3][2] = 0.0;
    this.m_tranData[3][3] = 1.0;
    this.SetRotationRPY(rx, ry, rz);
    this.SetTranslation(x, y, z);
  }

  internal void SetX(double x) => this.m_tranData[0][3] = x;

  internal void SetY(double y) => this.m_tranData[1][3] = y;

  internal void SetZ(double z) => this.m_tranData[2][3] = z;

  internal CApMFTransformation Multiply(double a)
  {
    this.m_tranData[0][0] *= a;
    this.m_tranData[0][1] *= a;
    this.m_tranData[0][2] *= a;
    this.m_tranData[0][3] *= a;
    this.m_tranData[1][0] *= a;
    this.m_tranData[1][1] *= a;
    this.m_tranData[1][2] *= a;
    this.m_tranData[1][3] *= a;
    this.m_tranData[2][0] *= a;
    this.m_tranData[2][1] *= a;
    this.m_tranData[2][2] *= a;
    this.m_tranData[2][3] *= a;
    this.m_tranData[3][0] *= a;
    this.m_tranData[3][1] *= a;
    this.m_tranData[3][2] *= a;
    this.m_tranData[3][3] *= a;
    return this;
  }

  internal CApMFTransformation Multiply(CApMFTransformation m)
  {
    CApMFTransformation mfTransformation = new CApMFTransformation();
    for (int index1 = 0; index1 < 4; ++index1)
    {
      for (int index2 = 0; index2 < 4; ++index2)
        mfTransformation.m_tranData[index1][index2] = this.m_tranData[index1][0] * m.m_tranData[0][index2] + this.m_tranData[index1][1] * m.m_tranData[1][index2] + this.m_tranData[index1][2] * m.m_tranData[2][index2] + this.m_tranData[index1][3] * m.m_tranData[3][index2];
    }
    return mfTransformation;
  }

  internal void SetTranslation(double x, double y, double z)
  {
    this.m_tranData[0][3] = x;
    this.m_tranData[1][3] = y;
    this.m_tranData[2][3] = z;
  }

  internal void SetRotationRPY(double rx, double ry, double rz)
  {
    double sin0;
    double cos0;
    double sin1;
    double cos1;
    double sin2;
    double cos2;
    this.SetSinCos(rx, ry, rz, out sin0, out cos0, out sin1, out cos1, out sin2, out cos2);
    this.m_tranData[0][0] = cos2 * cos1;
    this.m_tranData[0][1] = cos2 * sin1 * sin0 - sin2 * cos0;
    this.m_tranData[0][2] = cos2 * sin1 * cos0 + sin2 * sin0;
    this.m_tranData[1][0] = sin2 * cos1;
    this.m_tranData[1][1] = sin2 * sin1 * sin0 + cos2 * cos0;
    this.m_tranData[1][2] = sin2 * sin1 * cos0 - cos2 * sin0;
    this.m_tranData[2][0] = -sin1;
    this.m_tranData[2][1] = cos1 * sin0;
    this.m_tranData[2][2] = cos1 * cos0;
  }

  internal TxVector GetTranslationAsVector()
  {
    double x;
    double y;
    double z;
    this.GetTranslation(out x, out y, out z);
    return new TxVector(x, y, z);
  }

  internal void GetTranslation(out double x, out double y, out double z)
  {
    x = this.m_tranData[0][3];
    y = this.m_tranData[1][3];
    z = this.m_tranData[2][3];
  }

  internal TxVector GetRotationRPYAsVector()
  {
    double rx;
    double ry;
    double rz;
    this.GetRotationRPY(out rx, out ry, out rz);
    return new TxVector(rx, ry, rz);
  }

  internal void GetRotationRPY(out double rx, out double ry, out double rz)
  {
    double num1 = this.m_tranData[1][0];
    double num2 = this.m_tranData[0][0];
    double y1;
    double x1;
    double num3;
    if (num1 < 1E-06 && num1 > -1E-06)
    {
      if (num2 > -1E-06)
      {
        y1 = 0.0;
        x1 = 1.0;
        num3 = num2;
        if (num2 < 1E-06)
          num3 = 0.0;
      }
      else
      {
        y1 = 0.0;
        x1 = -1.0;
        num3 = -num2;
      }
    }
    else if (num2 < 1E-06 && num2 > -1E-06)
    {
      if (num1 > 1E-06)
      {
        y1 = 1.0;
        x1 = 0.0;
        num3 = num1;
      }
      else
      {
        y1 = -1.0;
        x1 = 0.0;
        num3 = -num1;
      }
    }
    else
    {
      num3 = Math.Sqrt(num1 * num1 + num2 * num2);
      y1 = num1 / num3;
      x1 = num2 / num3;
    }
    double num4 = -this.m_tranData[2][0];
    double num5 = num3;
    double y2;
    double x2;
    if (num4 < 1E-06 && num4 > -1E-06)
    {
      y2 = 0.0;
      x2 = 1.0;
    }
    else if (num5 < 1E-06 && num5 > -1E-06)
    {
      if (num4 > 1E-06)
      {
        y2 = 1.0;
        x2 = 0.0;
      }
      else
      {
        y2 = -1.0;
        x2 = 0.0;
      }
    }
    else
    {
      double num6 = Math.Sqrt(num4 * num4 + num5 * num5);
      y2 = num4 / num6;
      x2 = num5 / num6;
    }
    double num7 = y1 * this.m_tranData[0][2] - x1 * this.m_tranData[1][2];
    double num8 = x1 * this.m_tranData[1][1] - y1 * this.m_tranData[0][1];
    double y3;
    double x3;
    if (num7 < 1E-06 && num7 > -1E-06)
    {
      if (num8 > -1E-06)
      {
        y3 = 0.0;
        x3 = 1.0;
      }
      else
      {
        y3 = 0.0;
        x3 = -1.0;
      }
    }
    else if (num8 < 1E-06 && num8 > -1E-06)
    {
      if (num7 > 1E-06)
      {
        y3 = 1.0;
        x3 = 0.0;
      }
      else
      {
        y3 = -1.0;
        x3 = 0.0;
      }
    }
    else
    {
      double num9 = Math.Sqrt(num7 * num7 + num8 * num8);
      y3 = num7 / num9;
      x3 = num8 / num9;
    }
    rx = Math.Atan2(y3, x3);
    ry = Math.Atan2(y2, x2);
    rz = Math.Atan2(y1, x1);
  }

  internal CApMFTransformation Inverse()
  {
    CApMFTransformation mfTransformation = new CApMFTransformation();
    mfTransformation.m_tranData[0][0] = this.Det(1, 2, 3, 1, 2, 3);
    mfTransformation.m_tranData[1][0] = -this.Det(1, 2, 3, 0, 2, 3);
    mfTransformation.m_tranData[2][0] = this.Det(1, 2, 3, 0, 1, 3);
    mfTransformation.m_tranData[3][0] = -this.Det(1, 2, 3, 0, 1, 2);
    mfTransformation.m_tranData[0][1] = -this.Det(0, 2, 3, 1, 2, 3);
    mfTransformation.m_tranData[1][1] = this.Det(0, 2, 3, 0, 2, 3);
    mfTransformation.m_tranData[2][1] = -this.Det(0, 2, 3, 0, 1, 3);
    mfTransformation.m_tranData[3][1] = this.Det(0, 2, 3, 0, 1, 2);
    mfTransformation.m_tranData[0][2] = this.Det(0, 1, 3, 1, 2, 3);
    mfTransformation.m_tranData[1][2] = -this.Det(0, 1, 3, 0, 2, 3);
    mfTransformation.m_tranData[2][2] = this.Det(0, 1, 3, 0, 1, 3);
    mfTransformation.m_tranData[3][2] = -this.Det(0, 1, 3, 0, 1, 2);
    mfTransformation.m_tranData[0][3] = -this.Det(0, 1, 2, 1, 2, 3);
    mfTransformation.m_tranData[1][3] = this.Det(0, 1, 2, 0, 2, 3);
    mfTransformation.m_tranData[2][3] = -this.Det(0, 1, 2, 0, 1, 3);
    mfTransformation.m_tranData[3][3] = this.Det(0, 1, 2, 0, 1, 2);
    double num = this.m_tranData[0][0] * mfTransformation.m_tranData[0][0] + this.m_tranData[0][1] * mfTransformation.m_tranData[1][0] + this.m_tranData[0][2] * mfTransformation.m_tranData[2][0] + this.m_tranData[0][3] * mfTransformation.m_tranData[3][0];
    if (num == 0.0)
    {
      mfTransformation.m_tranData[0][0] = mfTransformation.m_tranData[1][1] = mfTransformation.m_tranData[2][2] = mfTransformation.m_tranData[3][3] = 1.0;
      mfTransformation.m_tranData[0][1] = mfTransformation.m_tranData[0][2] = mfTransformation.m_tranData[0][3] = mfTransformation.m_tranData[1][0] = mfTransformation.m_tranData[1][2] = mfTransformation.m_tranData[1][3] = mfTransformation.m_tranData[2][0] = mfTransformation.m_tranData[2][1] = mfTransformation.m_tranData[2][3] = mfTransformation.m_tranData[3][0] = mfTransformation.m_tranData[3][1] = mfTransformation.m_tranData[3][2] = 0.0;
    }
    else
      mfTransformation.Multiply(1.0 / num);
    return mfTransformation;
  }

  private double Det(int i0, int i1, int i2, int j0, int j1, int j2)
  {
    return this.m_tranData[i2][j2] * (this.m_tranData[i0][j0] * this.m_tranData[i1][j1] - this.m_tranData[i0][j1] * this.m_tranData[i1][j0]) - this.m_tranData[i2][j1] * (this.m_tranData[i0][j0] * this.m_tranData[i1][j2] - this.m_tranData[i0][j2] * this.m_tranData[i1][j0]) + this.m_tranData[i2][j0] * (this.m_tranData[i0][j1] * this.m_tranData[i1][j2] - this.m_tranData[i0][j2] * this.m_tranData[i1][j1]);
  }

  private void SetSinCos(
    double rotX,
    double rotY,
    double rotZ,
    out double sin0,
    out double cos0,
    out double sin1,
    out double cos1,
    out double sin2,
    out double cos2)
  {
    sin0 = Math.Sin(rotX);
    cos0 = Math.Cos(rotX);
    sin1 = Math.Sin(rotY);
    cos1 = Math.Cos(rotY);
    sin2 = Math.Sin(rotZ);
    cos2 = Math.Cos(rotZ);
  }
}
